/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj.templates.subsystems;

import edu.wpi.first.wpilibj.AnalogChannel;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.Victor;
import edu.wpi.first.wpilibj.command.PIDSubsystem;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
import edu.wpi.first.wpilibj.templates.commands.MoveWristWithStick;
import edu.wpi.first.wpilibj.templates.commands.SetWristState;

/**
 *
 * @author Jarrod Risley
 */

public class DaisyWristSubsystem extends PIDSubsystem {

     /*
     * Global Class Variables
     */
    private DaisyWristSubsystem.PresetWristPosition currentPosition;
    private Victor leftIntakeVictor;
    private Victor rightIntakeVictor;
    private Victor wristVictor;
    private AnalogChannel wristPot;
    
    private static final double Kp = 0.005;
    private static final double Ki = 0.0;
    private static final double Kd = 0.0;
    
    private static final double MIN_IMPULSE_DURATION = 0.5;
    private static final double TIME_TO_TRAVERSE_COUNT = 0; //Calibrate!

    private String subsystemName;
    private double lastCommandTime;
    private double impulseDuration;
    private boolean motorsEnabled;

    
    /**
     * Class used for the two positions of this subsystem, since the wrist
     * should only be at one of to pre-determined locations.
     */
    
    public static class PresetWristPosition {
        
        /*
         * Inner Class Variables
         */
        private int value;
        private static final int DOWN = 749;
        private static final int UP = 291;
        private static final int MID = 400;
        
        public static final DaisyWristSubsystem.PresetWristPosition INGEST_POSITION =
                new DaisyWristSubsystem.PresetWristPosition(DOWN);
        
        public static final DaisyWristSubsystem.PresetWristPosition RAISED_POSITION = 
                new DaisyWristSubsystem.PresetWristPosition(UP);
        
        public static final DaisyWristSubsystem.PresetWristPosition MIDDLE_POSITION =
                new DaisyWristSubsystem.PresetWristPosition(MID);
        
        
        // INNER CLASS CONSTRUCTOR //
        
        
        private PresetWristPosition(int value) {
            this.value = value;
        } // end PresetWristPosition Constructor
        
        
        // INNER CLASS METHOD //
        
        
        /**
         * Gets the value of the position
         * 
         * @return The value of the position.
         */
        
        public int getValue() {
            return value;
        } // end getValue
        
    } // end PresetWristPosition
    
    
    // CONSTRUCTOR // 
    
    
    /**
     * Creates a DaisyWristSubsystem.
     * 
     * @param name The name of the subsystem.
     * @param sidecarNum The sidecar this subsystem is attached to.
     * @param intakePwmPort The port to which the Daisies are attached.
     * @param wristPwmPort The port to which the wrist motors.
     * @param potPort The wrist's potentiometer port.
     */
    
    public DaisyWristSubsystem(String name, int sidecarNum, int leftIntakePwmPort, 
            int rightIntakePwmPort, int wristPwmPort, int potPort) {
        
        super("DaisyWristSubsystem", Kp, Ki, Kd);
        
        System.out.println("Creating DaisyWristSubsystem " + name + "...");

        subsystemName = name;
        
        leftIntakeVictor = new Victor(sidecarNum, leftIntakePwmPort);
        rightIntakeVictor = new Victor(sidecarNum, rightIntakePwmPort);
        wristVictor = new Victor(sidecarNum, wristPwmPort);
        wristPot = new AnalogChannel(potPort);
        
        LiveWindow.run();
        LiveWindow.addActuator(subsystemName, "Left intake", leftIntakeVictor);
        LiveWindow.addActuator(subsystemName, "Right intake", rightIntakeVictor);
        LiveWindow.addActuator(subsystemName, "Wrist Joint", wristVictor);
        LiveWindow.addSensor(subsystemName, "Wrist Pot", wristPot);
        LiveWindow.addSensor(subsystemName, "PID Controller", getPIDController());
        
        
    } // end DaisyWristSubsystem Constructor
    
    
    // METHODS //
    
    
    // No Default Command for this subsystem.
    public void initDefaultCommand() {
        
        setDefaultCommand(new MoveWristWithStick());
    
    } // end initDefaultCommand
    
    /**
     * Used for the PIDController.
     * 
     * @return The average Pot Value.
     */
    
    protected double returnPIDInput() {
        // Return your input value for the PID loop
        // e.g. a sensor, like a potentiometer:
        // yourPot.getAverageVoltage() / kYourMaxVoltage;

        return wristPot.getAverageValue();
    } // end returnPIDInput
    
    protected void usePIDOutput(double output) {
        // Use output to drive your system, like a motor
        // e.g. yourMotor.set(output);
        
        output = -output * 0.75;
        
        wristVictor.set(output);
        
//        System.out.println("Wrist Pot Reads: " + wristPot.getAverageValue());
        
//        if ( (motorsEnabled) && (Timer.getFPGATimestamp() > lastCommandTime 
//                + impulseDuration)) {
//            
//            motorsEnabled = false;
//            disable();
//            
//        } // end if
        
    } // end usePIDOutput
    
    /**
     * Gets the current location of the wrist
     * 
     * @return The value of the current PresetWristPosition.
     */
    
    public int getLocation() {
//        return currentPosition.getValue();
        return wristPot.getAverageValue();
    } // end getLocation
    
    /**
     * Sets the position of the wrist. Generally speaking, this only should be
     * called for autonomous functions rather than set to a button.
     * 
     * @param position A PresetWristPosition.
     */
    
    public void setWristPosition(DaisyWristSubsystem.PresetWristPosition position) {
        
//        enable(); // turns the PIDController on.
        currentPosition = position;
        impulseDuration = MIN_IMPULSE_DURATION + TIME_TO_TRAVERSE_COUNT * (position.getValue() - getLocation());
        motorsEnabled = true;
        lastCommandTime = Timer.getFPGATimestamp();
        setSetpoint(position.getValue());
        
    } // end setWristPosition
    
    public void setWristPosition(double position) {
        
//        enable(); // turns the PIDController on.
        impulseDuration = MIN_IMPULSE_DURATION + TIME_TO_TRAVERSE_COUNT * (position - getLocation());
        motorsEnabled = true;
        lastCommandTime = Timer.getFPGATimestamp();
        setSetpoint(position);
        
    } // end setWristPosition
    
    /**
     * Activates the intake.
     * 
     * @param input Full on, off or reverse only.
     */
    
    public void intake(double input) {
        leftIntakeVictor.set(input/2);
        rightIntakeVictor.set(-input/2);
    } // end intake
    
    public void manualDriveWrist(double input) {
        
        
        wristVictor.set(input);
        
    } // end manualDriveWrist
    
} // end DaisyWristSubsystem
